/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include <math.h>
#include <stdlib.h>
/*#include "main.h"*/
#include "celpfilt.h"
#include "adapt.h"
#include "submult.h"
#include "del_tab.h"
#include "acb_parm.h"
#include "delay.h"
#include "bwexp.h"
#include "setarray.h"
#include "acb_code.h"
#include "movarray.h"
#include "con_adap.h"

#ifdef INTG
int	fraction = FALSE;
int	neigh = FALSE;
#else /* INTG */
#ifdef FULL
int	fraction = TRUE;
int	neigh = FALSE;
#else /* FULL -- 
	if no compile-time options are specified, the hierarchical 
	search below is used (FS1016 method) */
int	fraction = FALSE;
int	neigh = TRUE;
int	NeighborhoodRange = 3;
#endif /* FULL */
#endif /* INTG */

#define START 		B_PTR - SF_LEN + 1

/*  Length of truncated impulse response */
/*	IR values past 30 are nearly zero */
#define LEN_TRUNC_H	30

static void CalcPointers(
int 	even, 
int 	PrevDelayPtr, 
int 	*MinDelayPtr, 
int 	*MaxDelayPtr);

static void TopMatch(
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr, 
int 	*BestDelayPtr);

static void SelectDelay(
int 	*BestDelayPtr, 
int 	SubMult[256][4], 
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr);

static void Neighborhood(
float 	lp_imp[SF_LEN], 
float 	residual[RES_LEN], 
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr, 
float	DelayTable[],
float 	AdaptCB[MAX_ABUF_LEN], 
int 	*BestDelayPtr, 
float 	gain[MAX_NUM_ADAPT]);

static void CalcIntAdaptParms(
float	AdaptCB[MAX_ABUF_LEN],
float	pc_imp[SF_LEN],
int	MinDelayPtr,
int	MaxDelayPtr,
float	DelayTable[MAX_NUM_ADAPT],
float	residual[RES_LEN],
float	gain[MAX_NUM_ADAPT],
float	match[MAX_NUM_ADAPT]);

static void CalcFracAdaptParms(
float	AdaptCB[MAX_ABUF_LEN],
float	pc_imp[SF_LEN],
int	MinDelayPtr,
int	MaxDelayPtr,
float	DelayTable[MAX_NUM_ADAPT],
float	residual[RES_LEN],
float	gain[MAX_NUM_ADAPT],
float	match[MAX_NUM_ADAPT]);

static void FindAdaptResidual(
float	speech[SF_LEN],
float	AdaptCB[MAX_ABUF_LEN],
float	pc[ORDER+1],
float	AdaptGain,
float	AdaptDelay,
float	residual[RES_LEN]);

/*

***************************************************************************
*                                                                         *
* ROUTINE
*		AdaptiveAnalysis
*
* FUNCTION
*		Subframe adaptive codebook search
* SYNOPSIS
*		AdaptiveAnalysis(speech_in, pc, h, AdaptCB, 
*			residual_in, AdaptDelay, AdaptGain, residual_out)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	SpeechIn	float	 i	Subframe of speech
*	pc		float	 i	Predictor Coefficients (PCs)
*	pc_imp		float	 i	Impulse response of PCs
*	AdaptCB		float	 i	Adaptive codebook vector
*	residual_in	float	 i	Residual from LPC analysis
*	AdaptDelay	float	 o	pitch delay parameter to TX
*	AdaptGain	float	 o	pitch gain parameter to TX
*	residual_out	float	 o	residual remaining after comparison
*
**************************************************************************/

void AdaptiveAnalysis(
float 	SpeechIn[SF_LEN], 
float 	pc[ORDER], 
float 	pc_imp[SF_LEN], 
float 	AdaptCB[MAX_ABUF_LEN], 
float 	residual_in[RES_LEN], 
float 	*AdaptDelay, 
float 	*AdaptGain, 
float 	residual_out[RES_LEN])
{
	float	match[MAX_NUM_ADAPT], gain[MAX_NUM_ADAPT];
	int	MaxDelayPtr, MinDelayPtr, BestDelayPtr;
static 	int	even=FALSE, PrevDelayPtr=1, first=TRUE;
	int	index;
	float	TempAdaptCB[B_PTR];
#ifdef NEW_ACB
	int	i, Delay;
	float	pcexp[ORDER+1];    /* !!OUTOFBOUNDS!! without the '+1' */
#endif

/*  Update temporary adaptive codebook */
	SetArray(B_PTR, 0.0, TempAdaptCB);
	MoveArray(ACB_SIZE, AdaptCB, &TempAdaptCB[B_PTR-(ACB_SIZE)-SF_LEN]);

/*  Set up initial conditions on first subframe */
	if (first)	{
	  first = FALSE;
	  *AdaptGain = 0.0;
	  *AdaptDelay = MIN_DELAY;
	}

/*  Calculate adaptive gain and delay for subsequent subframes */
	else	{

  
/*  Determine allowable pointer range for full/delta searches on */
/*	even/odd subframes */
	  CalcPointers(even, PrevDelayPtr, &MinDelayPtr, &MaxDelayPtr);

/*  Initialize arrays */
	  SetArray(MAX_NUM_ADAPT, 0.0, match);
	  SetArray(MAX_NUM_ADAPT, 0.0, gain);

/*  Calculate gain and match scores for integer adaptive delays */
	  CalcIntAdaptParms(TempAdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr, 
		DelayTable, residual_in, gain, match);

	  if (fraction)
/*  Calculate gain and match scores for fractional delays */
	    CalcFracAdaptParms(TempAdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr, 
		DelayTable, residual_in, gain, match);

/*  Determine top match score from those computed */
	  TopMatch(match, MinDelayPtr, MaxDelayPtr, &BestDelayPtr);

/*  Select shortest delay */
	  if(!even)
	    SelectDelay(&BestDelayPtr, submult, match, MinDelayPtr, 
		MaxDelayPtr);

	  if(neigh)	{
/*  Find best neighborhood match */
	    Neighborhood(pc_imp, residual_in, match, MinDelayPtr, MaxDelayPtr, 
		DelayTable, TempAdaptCB, &BestDelayPtr, gain);
	  }

/*  Assign pitch (adaptive codebook) parameters */
	  *AdaptGain = gain[BestDelayPtr];
	  *AdaptDelay = DelayTable[BestDelayPtr];

/*  Save pointer to detrmine delta delay */
	  PrevDelayPtr = BestDelayPtr;

/*  Encode adaptive gain (quantize) */
	  *AdaptGain = ACBGainEncode(*AdaptGain, &index);

#ifdef NEW_ACB
/*  ACB contribution to LP Impulse Response */
	  if(*AdaptDelay < SF_LEN)	{
/*  Lay out pulses at multiples of the pitch period	*/
	    Delay = (int)(*AdaptDelay);
	    SetArray(SF_LEN, 0.0, pc_imp);
	    pc_imp[0] = 1.0;
	    for(i=Delay;i<SF_LEN;i+=Delay)
	      pc_imp[i] = *AdaptGain;
/*  Filter the new signal with expanded PCs (no history on filter)	*/
	    BWExpand(GAMMA, pc, pcexp);
	    FilterImpulseResponse(pc_imp, pcexp);
	  }
#endif
	}

/*  Calculate residual after pitch analysis */
	SetArray(B_PTR, 0.0, TempAdaptCB);
	MoveArray(ACB_SIZE, AdaptCB, TempAdaptCB);

	FindAdaptResidual(SpeechIn, TempAdaptCB, pc, *AdaptGain, *AdaptDelay, 
		residual_out);

/*  Update values for next subframe */
	even = (even) ? FALSE : TRUE;

}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CalcPointers
*
* FUNCTION
*		Calculate allowable pointer range
* SYNOPSIS
*		CalcPointers(even, oldptr, minptr, maxptr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	even		int	 i	Even/Odd subframe marker
*	PrevDelayPtr	int	 i	Previous delay pointer
*	MinDelayPtr	int	 o	Pointer to minimum delay
*	MaxDelayPtr	int	 o	Pointer to maximum delay
*
**************************************************************************/
void CalcPointers(
int 	even, 
int 	PrevDelayPtr, 
int 	*MinDelayPtr, 
int 	*MaxDelayPtr)
{

/*  Delta delay coding on even subframes */
	if(even)	{
	  *MinDelayPtr = PrevDelayPtr - (NUM_DELTA_ADELAYS / 2 - 1);
	  *MaxDelayPtr = PrevDelayPtr + (NUM_DELTA_ADELAYS / 2);
	  if (*MinDelayPtr < 0)	{
	    *MinDelayPtr = 0;
	    *MaxDelayPtr = NUM_DELTA_ADELAYS - 1;
	  }
	  if (*MaxDelayPtr > NUM_FULL_ADELAYS-1)	{
	    *MaxDelayPtr = NUM_FULL_ADELAYS - 1;
	    *MinDelayPtr = NUM_FULL_ADELAYS - NUM_DELTA_ADELAYS;
	  }
	}
/*  Full range coding on odd subframes */
	else	{
	  *MinDelayPtr = 0;
	  *MaxDelayPtr = NUM_FULL_ADELAYS - 1;
	}

}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		TopMatch
*
* FUNCTION
*		Find pointer to top (MSPE) match score (BestDelayPtr)
*		search for best match score (max -error term)
* SYNOPSIS
*		TopMatch(match, MinDelayPtr, MaxDelayPtr, BestDelayPtr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	match		float	 i	Match score array
*	MinDelayPtr	int	 i	Pointer to minimum delay to search
*	MaxDelayPtr	int	 i	Pointer to maximum delay to search
*	BestDelayPtr	int	 o	Pointer to best delay
*
**************************************************************************/

void TopMatch(
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr, 
int 	*BestDelayPtr)
{
int	i;
float	emax;

	*BestDelayPtr = MinDelayPtr;

	emax = match[*BestDelayPtr];

	for(i=MinDelayPtr; i<=MaxDelayPtr; i++)	{
	  if (match[i] > emax)	{
	    *BestDelayPtr = i;
	    emax = match[*BestDelayPtr];
	  }
	}
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		SelectDelay
*
* FUNCTION
*		Select shortest delay of 2, 3, or 4 submultiples
*		if its match is within 1 dB of MSPE to favor
		smooth "pitch"
* SYNOPSIS
*		SelectDelay(BestDelayPtr, SubMult, match, 
*		              MinDelayPtr, MaxDelayPtr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	BestDelayPtr	int	i/o	Pointer to delay with top match score
*	SubMult		int	 i	Sumbmultiples array
*	match		float	 i	Match score array for each delay
*	MinDelayPtr	int	 i	Pointer to minimum delay to search
*	MaxDelayPtr	int	 i	Pointer to maximum delay to search
*
**************************************************************************/
void SelectDelay(
int 	*BestDelayPtr, 
int 	SubMult[256][4], 
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr)
{
int	i;
int	SubMultPtr, BigPtr;
int	Best;

/*  Initialize */
	Best = *BestDelayPtr;

/*  For each submultiple (2, 3, & 4) */
	for(i=1; i<=SubMult[*BestDelayPtr][0]; i++)	{

/*  Find best neighborhood match for given submultiple */
	  BigPtr = SubMult[*BestDelayPtr][i];
	  for(SubMultPtr = max(SubMult[*BestDelayPtr][i]-8, MinDelayPtr);
		SubMultPtr <= min(SubMult[*BestDelayPtr][i]+8, MaxDelayPtr); 
		SubMultPtr++)	{
	    if(match[SubMultPtr] > match[BigPtr])
	      BigPtr = SubMultPtr;
	  }

/*  Select submultiple match if within 1 dB MSPE match */
	  if(match[BigPtr] >= (0.88*match[*BestDelayPtr]))
	    Best = BigPtr;

	}

	*BestDelayPtr = Best;
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		Neighborhood
*
* FUNCTION
*		Search BestDelayPtr's neighboring delays (to be used with 
*		earlier stages of searching).  Find gain and match 
*		score for neighboring delays and find best neighborhood 
*		match (could use end-point correction on unity spaced delays)
* SYNOPSIS
*		Neighborhood(lp_imp, residual, match, MinDelayPtr, MaxDelayPtr
				DelayTable, AdaptCB, BestDelayPtr, gain)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	lp_imp		float	 i	LP impulse response
*	residual	float	 i	Residual from LPC analysis
*	match		float	 i	Adaptive Codebook match score array
*	MinDelayPtr	int	 i	Pointer to minimum delay to search
*	MaxDelayPtr	int	 i	Pointer to maximum delay to search
*	DelayTable	float	 i	Table of integer and fractional delays
*	AdaptCB		float	 i	Adaptive codebook
*	BestDelayPtr	int	 o	Pointer to best delay
*	gain		float	 o	Adaptive Codebook gain array
*
**************************************************************************/
void Neighborhood(
float 	lp_imp[SF_LEN], 
float 	residual[RES_LEN], 
float 	match[MAX_NUM_ADAPT], 
int 	MinDelayPtr, 
int 	MaxDelayPtr, 
float	DelayTable[],
float 	AdaptCB[MAX_ABUF_LEN], 
int 	*BestDelayPtr, 
float 	gain[MAX_NUM_ADAPT])
{
int	i;
int	BigPtr;
int	IntDelay;
float	FracDelay;
float	AdaptCBShift[MAX_A_LEN];

	BigPtr = *BestDelayPtr;

	for(i=max(*BestDelayPtr - NeighborhoodRange, MinDelayPtr); 
		i<=min(*BestDelayPtr + NeighborhoodRange, MaxDelayPtr); i++){
	  if (i != *BestDelayPtr)	{
	    IntDelay = (int)(DelayTable[i]);
	    FracDelay = DelayTable[i] - IntDelay;
	    if(fabs(FracDelay) < .0001)	
/*	      CalcACBParms(&AdaptCB[START - IntDelay-1], SF_LEN, lp_imp, 
		TRUE, IntDelay, LEN_TRUNC_H, residual, &match[i], &gain[i]);
*/;
	    else	{
	      delay(AdaptCB, B_PTR, START, SF_LEN, FracDelay, IntDelay, -4, 3, 
		5,  &AdaptCBShift[0]);
	      CalcACBParms(AdaptCBShift, SF_LEN, lp_imp, TRUE, 69, 
		LEN_TRUNC_H, residual, &match[i], &gain[i]);
	    }
	    if (match[i] > match[*BestDelayPtr])
	      BigPtr = i;
	  }
	}
	
	*BestDelayPtr = BigPtr;
}

/*

*************************************************************************
*                                                                         
* ROUTINE
*		CalcIntAdaptParms
*
* FUNCTION
*		Calculate gain and match score for integer adaptive delays
* SYNOPSIS
*		CalcIntAdaptParms(AdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr, *			DelayTable, residual, gain, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	AdaptCB		float	 i	Adaptive Codebook
*	pc_imp		float	 i	Impulse response of PCs
*	MinDelayPtr	int	 i	Pointer to minimum delay to search
*	MaxDelayPtr	int	 i	Pointer to maximum delay to search
*	DelayTable	float	 i	Adaptive delay coding table
*	residual	float	 i	Residual from LPC analysis
*	gain		float	 o	Gain array
*	match		float	 o	Match score array
*
**************************************************************************
*
*	Uses end-point correction on unity spaced delays
*
**************************************************************************/
void CalcIntAdaptParms(
float	AdaptCB[MAX_ABUF_LEN],
float	pc_imp[SF_LEN],
int	MinDelayPtr,
int	MaxDelayPtr,
float	DelayTable[MAX_NUM_ADAPT],
float	residual[RES_LEN],
float	gain[MAX_NUM_ADAPT],
float	match[MAX_NUM_ADAPT])
{
int	first = TRUE;
int	IntDelay;
float	FracDelay;
int	i;


	for(i=MinDelayPtr;i<=MaxDelayPtr;i++)	{
	  IntDelay = (int)(DelayTable[i]);
	  FracDelay = DelayTable[i] - IntDelay;
	  if (fabs(FracDelay) < .0001)	{
	    CalcACBParms(&AdaptCB[START - IntDelay-1], SF_LEN, pc_imp,
		first,IntDelay, LEN_TRUNC_H, &residual[0], &match[i], &gain[i]);
	    first = FALSE;
	  }
	  else
	    match[i] = 0.0;
	}

}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CalcFracAdaptParms
*
* FUNCTION
*		Calculate gain and match score for fractional adaptive delays
* SYNOPSIS
*		CalcFracAdaptParms(AdaptCB, pc_imp, MinDelayPtr, MaxDelayPtr, 
*			DelayTable, residual, gain, match)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	AdaptCB		float	 i	Adaptive Codebook
*	pc_imp		float	 i	Impulse response of PCs
*	MinDelayPtr	int	 i	Minimum delay pointer to search
*	MaxDelayPtr	int	 i	Maximum delay pointer to search
*	DelayTable	float	 i	Adaptive delay coding table
*	residual	float	 i	Residual from LPC analysis
*	gain		float	 o	Gain array
*	match		float	 o	Match score array
*
**************************************************************************
*
*	Uses end-point correction on unity spaced delays
*
**************************************************************************/
void CalcFracAdaptParms(
float	AdaptCB[MAX_ABUF_LEN],
float	pc_imp[SF_LEN],
int	MinDelayPtr,
int	MaxDelayPtr,
float	DelayTable[MAX_NUM_ADAPT],
float	residual[RES_LEN],
float	gain[MAX_NUM_ADAPT],
float	match[MAX_NUM_ADAPT])
{
int	IntDelay;
double	FracDelay;
int	i;
float	AdaptCBShift[MAX_A_LEN];


	for(i=MinDelayPtr;i<=MaxDelayPtr;i++)	{
	  IntDelay = (int)(DelayTable[i]);
	  FracDelay = DelayTable[i] - IntDelay;
	  if (fabs(FracDelay) >= .0001)	{
	    delay(AdaptCB, B_PTR, START, SF_LEN, FracDelay, IntDelay, -4, 3, 5, 
			AdaptCBShift);
	    CalcACBParms(AdaptCBShift, SF_LEN, pc_imp, 1, 69, LEN_TRUNC_H, 
			residual, &match[i], &gain[i]);
	  }
	}

}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		FindAdaptResidual
*
* FUNCTION
*		Find residual after adaptive analysis
* SYNOPSIS
*		FindAdaptResidual(speech, AdaptCB, pc, AdaptGain, AdaptDelay, 
*			residual)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	speech		float	 i	Subframe of input speech
*	AdaptCB		float	 i	Adaptive Codebook vector
*	pc		float	 i	Subframe of interpolated predictor
*					coefficients
*	AdaptGain	float 	 i	Adaptive codebook gain
*	AdaptDelay	float	 i	Adaptive codebook delay
*	residual	float	 o	Adaptive residual
*
**************************************************************************/

void FindAdaptResidual(
float	speech[SF_LEN],
float	AdaptCB[MAX_ABUF_LEN],
float	pc[ORDER+1],
float	AdaptGain,
float	AdaptDelay,
float	residual[RES_LEN])
{
int	i;
float	pcexp[ORDER+1];

/*  Initialize residual */
	SetArray(RES_LEN, 0.0, residual);

/*  Adaptive Codebook Synthesis */
	ConstructAdaptCW(residual, SF_LEN, AdaptCB, ACB_SIZE, AdaptGain, 
		AdaptDelay, "long");

/*  LP Filter */
	do_pfilt_dynamic(&Adapt_ResP, pc, residual);

/*  Calculate Residual */
	for(i=0;i<RES_LEN;i++)	{
	  residual[i] = speech[i] - residual[i];
	}

/*  Perceptual Weighting of residual */
	do_zfilt_dynamic(&Adapt_ResZ, pc, residual);
	BWExpand(GAMMA, pc, pcexp);
	do_pfilt_dynamic(&Adapt_ResP2, pcexp, residual);


}
